#include <Motor.h>

Motor::Motor()
{
	lForward = 2500;
	lReverse = 500;
	rForward = 500;
	rReverse = 2500;
	lStop = 1400;
	rStop = 1400;
}

void Motor::forward()
{
    rightForward();
    leftForward();
}

void Motor::reverse()
{
    rightReverse();
    leftReverse();
}

void Motor::rightReverse()
{
    rightMotor.write(rReverse);
    rSpeed = rReverse;
}

void Motor::rightForward()
{
    rightMotor.write(rForward);
    rSpeed = rForward;
}

void Motor::leftReverse()
{
    leftMotor.write(lReverse);
    lSpeed = lReverse;
}

void Motor::leftForward()
{
    leftMotor.write(lForward);
    lSpeed = lForward;
}

void Motor::turnLeft()
{
    leftReverse();
    rightForward();
    rightForward();
}

void Motor::turnLeft(int time)
{
    leftReverse();
    rightForward();
    delay(time);
    rightForward();
}
        
void Motor::turnRight()
{
    rightReverse();
    leftForward();
    leftForward();
}

void Motor::turnRight(int time)
{
    rightReverse();
    leftForward();
    delay(time);
    leftForward();
}

void Motor::stop()
{
    leftMotor.write(lStop);
    lSpeed = lStop;
    rightMotor.write(rStop);
    rSpeed = rStop;
}

void Motor::setLeftForward(int l)
{
	lForward = l;
}
void Motor::setRightForward(int r)
{
	rForward = r;
}
void Motor::setLeftReverse(int l)
{
	lReverse = l;
}
void Motor::setRightReverse(int r)
{
	rReverse = r;
}
void Motor::setRightStop(int r)
{
	rStop = r;
}
void Motor::setLeftStop(int l)
{
	lStop = l;
}

int Motor::getLeftForward()
{
	return lForward;
}
int Motor::getRightForward()
{
	return rForward;
}
int Motor::getLeftReverse()
{
	return lReverse;
}
int Motor::getRightReverse()
{
	return rReverse;
}
int Motor::getRightStop()
{
	return rStop;
}
int Motor::getLeftStop()
{
	return lStop;
}

int Motor::getPulse(int angle)
{
  int pulse;
  pulse = map(angle, 0, 180, 544, 2400);
  
  return pulse;
}

void Motor::rightInit(int pin)
{
    rightMotor.attach(pin);
}

void Motor::leftInit(int pin)
{
    leftMotor.attach(pin);
}

void Motor::setSpeed(int val, char side)
{
    switch(side)
    {
        case 'l':
            leftMotor.write(getPulse(val));
            lSpeed = val;
            break;
        case 'r':
            rightMotor.write(getPulse(val));
            rSpeed = val;
            break;
    }
}

int Motor::getSpeed(char side)
{
    switch(side)
    {
        case 'l':
            return lSpeed;
            break;
        case 'r':
            return rSpeed;
            break;
    }
}